#include "algorithm.h"
#include <math.h>

static const char *TAG = "Algorithm";


float pidController::update(float error, float dt) {
  // 检查输入有效性
  if (isnan(error)) {
    LOGI(TAG, "error is nan");
    return 0;
  } else if (isinf(error)) {
    LOGI(TAG, "error is inf");
    return 0;
  } else if (dt <= 0) {
    LOGI(TAG, "dt is %f", dt);
    return 0;
  }
  // === 比例项 ===
  float P = Kp * error;

  // === 积分项（梯形积分）===
  float integral_increment = 0.5f * (error + prev_error) * dt;
  integral += integral_increment;
  //LOGI(TAG,"the integral is %f", integral);
  float I = Ki * integral;

  // === 微分项（带低通滤波）===
  float derivative = 0.0f;
  if (dt > 1e-6f) { // 避免除以零
    derivative = (error - prev_error) / dt;
  }

  // 应用一阶低通滤波
  float alpha = dt / (tau + dt);
  float filtered_derivative =
      (1.0f - alpha) * prev_derivative + alpha * derivative;
  float D = Kd * filtered_derivative;

  // === PID 输出 ===
  float output = P + I + D;

  // === 输出限幅和抗积分饱和 ===
  if (fabsf(output) > max_output) {
    output = (output > 0) ? max_output : -max_output;
    // 抗饱和：回退当前步长的积分增量
    integral -= integral_increment;
  }

  // 更新状态
  prev_derivative = filtered_derivative;
  prev_error = error;

  return output;
}